import torch


def cross_mat(lines: torch.TensorType):
    """
    生成直线对构成的双线性矩阵
    Arguments:
      - `lines`: 直线对的列向量组，$n\times 6$的列向量组，至少有5组
    """

    def cross_line(line: torch.TensorType):
        """
        生成直线对构成的双线性向量
        Arguments:
          - `line`: 直线对的列向量，6维列向量
        """
        return torch.tensor(
            [
                line[0] * line[3],  # a_1
                (line[0] * line[4] + line[1] * line[3]) / 2,  # a_2
                line[1] * line[4],  # a_3
                (line[0] * line[5] + line[2] * line[3]) / 2,  # b_1
                (line[1] * line[5] + line[2] * line[4]) / 2,  # b_2
                line[2] * line[5],  # c
            ],
            device=line.device,
        )

    try:
        assert lines.shape[0] >= 5
        return torch.stack([cross_line(line) for line in lines])
    except:
        raise ValueError("Please choose enough crossing lines first...")


def H_solver(cross_mat: torch.TensorType):
    """
    计算射影变换矩阵
    """

    _, _, Vh = torch.linalg.svd(cross_mat)
    b = Vh[-1, :]
    C_infty = torch.tensor(
        [
            [b[0], b[1] / 2, b[3] / 2],
            [b[1] / 2, b[2], b[4] / 2],
            [b[3] / 2, b[4] / 2, b[5]],
        ],
        device=cross_mat.device,
    )
    K_K_T = C_infty[0:2, 0:2]
    try:
        K_ = torch.linalg.cholesky(K_K_T, upper=True)
    # if torch.where(torch.isnan(K_) == True)[0].shape[0] > 0:
    except:
        K_ = torch.linalg.cholesky(-K_K_T, upper=True)
    K_ = K_ / torch.sqrt(torch.det(K_))

    K_K_T_v = torch.tensor([[b[3] / 2], [b[4] / 2]], device=cross_mat.device)
    v = torch.inverse(K_K_T) @ K_K_T_v

    H_ = torch.tensor(
        [[K_[0, 0], -K_[0, 1], 0], [-K_[1, 0], K_[1, 1], 0], [-v[0], -v[1], 1]],
        device=cross_mat.device,
    )
    print(f"H_rect:{H_}")
    print(f"H_esti:{torch.inverse(H_)}")
    return H_


def vector_product(vec_1, vec_2):
    return torch.tensor(
        [
            vec_1[1] * vec_2[2] - vec_1[2] * vec_2[1],
            vec_1[2] * vec_2[0] - vec_1[0] * vec_2[2],
            vec_1[0] * vec_2[1] - vec_1[1] * vec_2[0],
        ],
        device=vec_1.device,
    )
